/**
 * @FilePath     : \simulations\agvNavigate\helpSrc\force_relocate.cpp
 * @Description  : 实现了ForceRelocate类，用于管理和控制强制重定位的过程。
 *                 它包括重置状态、准备重定位、触发事件、检查触发器、等待完成、
 *                 设置阶段以及管理轨迹查找等功能。通过这些方法，可以有效地控制和监控重定位的各个阶段和状态。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
 * @LastEditTime : 2024-12-19 18:48:18
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "force_relocate.h"
/**
 * @brief 构造函数
 *
 * 调用 reset 方法初始化 ForceRelocate 对象
 */
ForceRelocate::ForceRelocate()
{
    Reset();
}

/**
 * @brief 重置强制重定位状态
 *
 * 重置强制重定位对象的状态，包括重定位标识、重定位阶段、触发器状态和下一个轨迹查找标志。
 */
void ForceRelocate::Reset()
{
    last_trig_id_ = trig_id_ = 0;
    SetStage(RelocateStage::RelocateUnknown);
    trig_.Stop();
    SetNextTrajFind(false);
}

/**
 * @brief 准备重定位
 *
 * 准备重定位操作，包括触发计数、设置重定位阶段、停止定时器、设置检测下一个路径标识为无效状态。
 */
void ForceRelocate::Ready()
{
    // 触发计数，设置重定位件阶段，定时器关闭，设置检测下一个路径标识为无效状态
    Trigger();
    SetStage(RelocateStage::RelocateUnknown);
    trig_.Stop();
    SetNextTrajFind(false);
}

/**
 * @brief 触发ForceRelocate事件
 *
 * 调用此方法将触发ForceRelocate事件，并返回当前触发事件的ID。
 * 如果trig_id_超过99999999，则会重置为1。
 *
 * @return 返回当前触发事件的ID
 */
int ForceRelocate::Trigger()
{
    if (++trig_id_ > 99999999) {
        trig_id_ = 1;
    }
    DEBUG_OUT("ForceRelocate trigger trig_id_=" << trig_id_);
    return trig_id_;
}

/**
 * @brief 强制重新定位一次触发器
 *
 * 检查当前触发器ID是否与上一次触发器ID不同，如果不同，则重新定位一次触发器。
 *
 * @return 如果触发器ID不同，则返回true，否则返回false。
 */
bool ForceRelocate::RelocateTrigOnce()
{
    bool ret = (last_trig_id_ != trig_id_);
    DEBUG_OUT("setRelocateOnce last_trig_id_ " << last_trig_id_ << ", cur_trig_id_:" << trig_id_);
    last_trig_id_ = trig_id_;
    ROSNode()->set_parameter({"forceRelocate", trig_id_});
    trig_.Stop();
    trig_.Start();
    return ret;
}

/**
 * @brief 等待重新定位完成
 *
 * 等待重新定位完成，如果重新定位已经完成，则返回true；否则，返回false。
 *
 * @param time_diff 时间差，单位：毫秒
 * @return 如果重新定位已经完成，则返回true；否则，返回false。
 */
bool ForceRelocate::WaitRelocateFinish(uint16_t time_diff)
{
    return trig_.TriggerAlways(time_diff);
}

/**
 * @brief 设置重新定位的阶段
 *
 * 设置重新定位的阶段，并将此阶段记录到内部变量中。
 *
 * @param stage 重新定位的阶段，类型为RelocateStage
 */
void ForceRelocate::SetStage(RelocateStage stage)
{
    stage_  = stage;
    DEBUG_OUT("ForceRelocate: SetStage=" << static_cast<int>(stage));
}

/**
 * @brief 获取当前阶段
 *
 * 获取ForceRelocate对象的当前阶段值。
 *
 * @return 当前阶段值
 */
RelocateStage ForceRelocate::GetStage() const
{
    return stage_;
}

/**
 * @brief 设置是否继续寻找下一个轨迹
 *
 * 设置是否继续寻找下一个轨迹。如果设置为true，则表示继续寻找下一个轨迹；
 * 如果设置为false，则表示停止寻找下一个轨迹。
 *
 * @param find 是否继续寻找下一个轨迹，true表示继续寻找，false表示停止寻找
 */
void ForceRelocate::SetNextTrajFind(bool find)
{
    find_next_ = find;
    DEBUG_OUT("ForceRelocate: SetNextTrajFind=" << find);
}

/**
 * @brief 查找下一个轨迹
 *
 * 查找并返回下一个轨迹的布尔值。
 *
 * @return 如果找到下一个轨迹，则返回true；否则返回false。
 */
bool ForceRelocate::NextTrajFind() const
{
    return find_next_;
}